#include<SoftwareSerial.h>

// Pins 2 and 7 are reserved for FTDI comms
// Pins 0 and 1 are reserved for USB comms (during programming only)

#define TRXRATE 19200
#define TERMBAUD 9600
#define LEDPIN 3
#define SENSORPIN A5
#define SPKRPIN 11
#define RX_boards 19
#define TX_boards 18
#define MAX_BUFFER_SIZE 64
#define TIME_OUT 25
#define MSG_RCVD 102
#define ID_REQUEST 103
#define SETUP_ID 104
#define SENDING 0
#define READING 1
#define COMPLETE 2
#define CONFIRM_IDSET 3

//COMMAND CODES:
#define COM 170
#define ROGER 171
#define NEGATIVE 172
#define SETID 173
#define SENDID 174
#define SETNEIGHBORS 175
#define IDSET 176
#define NEIGHBORSSET 177
#define NOCOM 255

//SoftwareSerial board_port(RX_1,TX_1);

int active_channel;
int listen_count;
int state;
long prevTime;
int ledState;
uint8_t myID;
uint8_t board_ID;
uint8_t read_buf[MAX_BUFFER_SIZE];
uint8_t write_buf[MAX_BUFFER_SIZE];
int read_buf_len;
int write_buf_len;
int send_status;
int next_available_ID;
int net_map[16][5];
int map_cur_row;


void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

void writebuffertoboards(uint8_t *buf, int buf_len){
    const uint8_t start_sig = COM;
    int temp_len = buf_len;
    uint8_t temp_buf[buf_len+1];
    while(temp_len > 0){
        temp_buf[temp_len] = buf[temp_len-1];
        temp_len--;
    }
    temp_buf[0] = start_sig;
    Serial.print("Writing");
    for(int i=0;i<buf_len+1;i++){
      Serial.print(", ");
      Serial.print(temp_buf[i], DEC);
    }
    Serial.println(" to serial the board network.");
    Serial1.write(temp_buf, buf_len+1);
}
void read_from_boards(){
  if(Serial1.read() == COM){
    int cnt=0;
    uint8_t cur_read = -1;
    while(Serial1.available() > 0 && cnt < MAX_BUFFER_SIZE && cur_read != COM){
      cur_read = Serial1.read();
      read_buf[cnt] = cur_read;
      cnt++;
    }
    read_buf_len = cnt;
    send_msg_received();
  }else{
    Serial1.flush();
  }
}

int send_msg(){
  int count = 0;
  int confirm_rcvd = 0;
  int temp_buf_len = 0;
  uint8_t temp_buf[MAX_BUFFER_SIZE];
  while(confirm_rcvd != 1 && count < TIME_OUT){
    writebuffertoboards(write_buf, write_buf_len);
    delay(30);
    if(Serial1.read() == COM){
        int cnt = 0;
        while(Serial1.available() > 0 && Serial1.peek() != COM){
            temp_buf[cnt] = Serial1.read();
            cnt++;
        }
        temp_buf_len = cnt;
        if(cnt > 0 && temp_buf[cnt-1] == ROGER){
          confirm_rcvd = 1;
          Serial.println("...");
        }else{
          while(Serial1.available() > 0)Serial1.read();
        }
    }else{
        //if the input buffer doesn't start with '170', flush the garbage.
        read_buf_len = 0;
        while(Serial1.available() > 0)Serial1.read();
    }
    count++;
  }
  delay(60);
  while(Serial1.available() > 0)Serial1.read();
  read_buf_len = 0;
  
  if(count >= TIME_OUT || temp_buf_len <= 0){
    return -1;
  }else{
    return 0;
  }
}

void send_msg_received(){
    uint8_t msg_rcvd = ROGER;
    for(int i=0;i<2;i++){
        writebuffertoboards(&msg_rcvd, 1);
        delayMicroseconds(500);
    }
    //returning with the assumption the our confirmation was received
}


void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  Serial1.begin(TRXRATE);
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  //Serial.println("Generating random ID...");
  myID = 0;
  board_ID = -1;
  active_channel = 1;
  listen_count = 1;
  digitalWrite(LEDPIN, HIGH);
  state = SENDING;
  read_buf_len = 0;
  write_buf_len = 0;
  for(int j=0;j<16;j++){
    for(int k=0;k<5;k++){
      net_map[j][k] = -1;
    }
  }
  map_cur_row = 0;
  ledState = LOW;
  next_available_ID = 1;
  prevTime = millis();
  //send_status = -1;
}

void loop(){
  //write_buf[0] = 2;
  //write_buf[1] = 6;
  switch(state){
    case SENDING:
      write_buf[0] = SENDID;
      write_buf_len = 1;
      send_status = send_msg();
      if(send_status != 0){
      //something went wrong with board to board comms
        Serial.print("Error: status code = ");
        Serial.print(send_status);
        Serial.println(". Something went wrong with board-to-board comms!!!");
      }else{
        state = READING;
      }
      break;
    case READING:
      delay(1);
      if(Serial1.read() == COM){
        Serial.println("COM");
        //uint8_t cur_read = -1;
        int cnt = 0;
        while(Serial1.available() > 0 && cnt < MAX_BUFFER_SIZE && Serial1.peek() != COM){
          //cur_read = Serial1.read();
          read_buf[cnt] = Serial1.read(); //cur_read;
          cnt++;
          Serial.println("available");
          delay(1);
        }
        read_buf_len = cnt;
      }
      if(read_buf_len > 0){
        send_msg_received();
        Serial.println("Received message from boards:");
        for(int i=0;i<read_buf_len;i++){
          Serial.print(read_buf[i]);
        }
        Serial.println("");
        switch(read_buf[0]){
          case IDSET:
            board_ID = read_buf[1];
            next_available_ID = board_ID + 1;
            state = SETNEIGHBORS;
            break;
          case NEIGHBORSSET:
            if(read_buf_len == 7){
              for(int i=0;i<5;i++){
                net_map[map_cur_row][i] = read_buf[i+1];
              }
              map_cur_row++;
              next_available_ID = read_buf[6];
              state = COMPLETE;
            }else{
              //we didn't get the whole message.
              Serial.print("Read Buffer Length = ");
              Serial.println(read_buf_len);
            }
            break;
          case SENDID:
            send_status = -1;
            write_buf[0] = myID;
            write_buf_len = 1;
            send_status = send_msg();
            if(send_status != 0){
              //something went wrong with board to board comms
              Serial.print("Error: status code = ");
              Serial.print(send_status);
              Serial.println(". Something went wrong with board-to-board comms!!!");
            }else{
              state = READING;
            }
            break;
          default:
            board_ID = read_buf[0];
            state = SETID;
            break;
        }
      }
    //while(Serial1.available() > 0)Serial1.read();
    //Serial.println("%%%");
    break;
  case COMPLETE:
    Serial.println("Set Neighbors Successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("System map:");
    for(int j=0;j<map_cur_row;j++){
      for(int k=0;k<5;k++){
        Serial.print(net_map[j][k]);
        Serial.print(", ");
      }
      Serial.println("");
    }
    Serial.println("***** TEST COMPLETE *****");
    delay(9000);
    break;
  case SETID:
    Serial.println("ID request successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("***** TEST COMPLETE *****");
    write_buf[0] = SETID;
    write_buf[1] = next_available_ID;
    write_buf_len = 2;
    send_status = send_msg();
    if(send_status != 0){
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Something went wrong with board-to-board comms!!!");
    }else{
      state = READING;
    }
    break;
 case SETNEIGHBORS:
    Serial.println("Set ID successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is now: ");
    Serial.println(board_ID);
    Serial.println("***** TEST COMPLETE *****");
    write_buf[0] = SETNEIGHBORS;
    write_buf[1] = next_available_ID;
    write_buf[2] = myID;
    write_buf_len = 3;
    send_status = send_msg();
    if(send_status != 0){
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Something went wrong with board-to-board comms!!!");
    }else{
      state = READING;
    }
    break;
  }
}
